
#include "rtdeapi.h"
#include "rtde_socket.h"
#include "rtde_hash.h"
#include "rtdeutils.h"
#include <pthread.h>

uint8_t* RTDE_SDK_Version  = "1.0.3";

//获取SDK版本号
RtdeResult rtde_get_SDK_Version(uint8_t* version)
{
    RtdeResult result = RTDE_ERR_NONE;

    if(version == NULL)
    {
        result = RTDE_ERR_UNKNOW;
        return result;
    }
    memcpy(version, RTDE_SDK_Version, strlen(RTDE_SDK_Version));
    return result;
}

int RtdeHandleData_num = 0;
//创建 RTDE_IP_MAX_NUM 个 RtdeHandleData_t
RtdeHandleData_t RtdeHandleData[RTDE_IP_MAX_NUM];

RtdeResult rtde_create_handle(RtdeHandle *rtdeHandle,const char *ip)
{
    RtdeResult result = RTDE_ERR_NONE;

    if(RtdeHandleData_num >= RTDE_IP_MAX_NUM)
    {
        printf("RtdeHandleData_num is full\n");
        result = RTDE_ERR_UNKNOW;
        return result;
    }
    
    *rtdeHandle = (RtdeHandle)get_time_multiply11_sum();
    RtdeDatespace_t *rtde_dataspace = (RtdeDatespace_t *)malloc(sizeof(RtdeDatespace_t));
    if (rtde_dataspace == NULL)
    {
        printf("malloc failed\n");
        result = RTDE_ERR_UNKNOW;
        return result;
    }
    memset(rtde_dataspace, 0, sizeof(RtdeDatespace_t));

    RtdeHandleData[RtdeHandleData_num].rtdeHandle = *rtdeHandle;
    RtdeHandleData[RtdeHandleData_num].rtde_dataspace = rtde_dataspace;
    memcpy(RtdeHandleData[RtdeHandleData_num].RtdeIp, ip, strlen(ip));
    RtdeHandleData[RtdeHandleData_num].RtdespaceNum = RtdeHandleData_num;

    RtdeHandleData[RtdeHandleData_num].rtde_dataspace->rtdeHandle = *rtdeHandle;
    
    RtdeHandleData_num++;//连接数+1

#if DEBUG_PRINT
    printf("RtdeHandleData_num:%d\n", RtdeHandleData_num);
#endif

    return result;
}

RtdeDatespace_t* get_rtde_dataspace(RtdeHandle rtdeHandle)
{
    RtdeDatespace_t *rtde_dataspace = NULL;
    if(rtdeHandle == 0)
    {   
        printf("RtdeHandle is null\n");
        return NULL;
    }
    int i;
    for(i=0; i<RtdeHandleData_num; i++)
    {
        if(RtdeHandleData[i].rtdeHandle == rtdeHandle)
        {
            rtde_dataspace =  RtdeHandleData[i].rtde_dataspace;
        }
    }
    return rtde_dataspace;
}




RtdeResult rtde_create(RtdeHandle rtdeHandle,const char *ip)
{
    RtdeResult result = RTDE_ERR_NONE;

    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtdeHandle);
    if(rtde_dataspace == NULL)
    {
        printf("rtde_create---RtdeHandle not found\n");
        return RTDE_ERR_UNKNOW;
    }

    rtde_dataspace->Socket_fd.sock_io_control = socket_connect(ip,RTDE_IO_Control_Interface_Port);

    if (rtde_dataspace->Socket_fd.sock_io_control < 0)
    {
        printf("Failed to connect to IO control interface\n");
        result  = RTDE_ERR_UNKNOW;
    }
    rtde_dataspace->Socket_fd.sock_control = socket_connect(ip,RTDE_Control_Interface_Port);
    if (rtde_dataspace->Socket_fd.sock_control < 0)
    {
        printf("Failed to connect to control interface\n");
        result  = RTDE_ERR_UNKNOW;
    }
    rtde_dataspace->Socket_fd.sock_receive = socket_connect(ip,RTDE_Receive_Interface_Port);
    if (rtde_dataspace->Socket_fd.sock_receive < 0)
    {
        printf("Failed to connect to receive interface\n");
        result  = RTDE_ERR_UNKNOW;
    }

#if DEBUG_PRINT
    printf("socket connect ok\n");
#endif
    rtde_dataspace->rtde_recv_state = RTDE_RECV_STATE_IDLE;

    rtde_thread_init(rtdeHandle);//初始化线程
    
    return result;
}


RtdeResult rtde_protocol_version_request(RtdeHandle rtdeHandle,int64_t version)
{
    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtdeHandle);
    if(rtde_dataspace == NULL)
    {
        return RTDE_ERR_UNKNOW;
    }
    RtdeResult result = RTDE_ERR_NONE;


    *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]) = 3+8; //header+data   // 3+8
    rtde_dataspace->rtde_ioctrl_sendbuf[2] = RTDE_REQUEST_PROTOCOL_VERSION;
    *(uint64_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[3]) = version;   

    int ssize = socket_send(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_sendbuf, *(uint16_t *)&rtde_dataspace->rtde_ioctrl_sendbuf[0]);
    int rsize = socket_recv(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_sendbuf);

    result = (RtdeResult)rtde_dataspace->rtde_ioctrl_sendbuf[3];
    if(result)//result != 0
    {
        printf("Error: %d\n", result);
    }        

#if DEBUG_PRINT    
    printf("Protocol version request ok\n");
#endif    
    return result;
}


RtdeResult rtde_output_variables_get(RtdeHandle rtdeHandle,int offset,RtdeVar *rtdeVar)
{
    RtdeResult result = RTDE_ERR_NONE;
    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtdeHandle);
    if(rtde_dataspace == NULL)
    {
        return RTDE_ERR_UNKNOW;
    }

    int len,num;
    *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]) = 3+4;
    rtde_dataspace->rtde_ioctrl_sendbuf[2] = RTDE_GET_CONTROL_OUTPUTS;
    *(uint32_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[3]) = offset;

    int ssize = socket_send(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_sendbuf, *(uint16_t *)&rtde_dataspace->rtde_ioctrl_sendbuf[0]);
    int rsize = socket_recv(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_recvbuf);

    result = (RtdeResult)rtde_dataspace->rtde_ioctrl_recvbuf[3];
    if(rtde_dataspace->rtde_ioctrl_recvbuf[3] != 0)//result != 0
    {
        printf("rtde_output_variables_get Error: %d\n", rtde_dataspace->rtde_ioctrl_recvbuf[3]);
        return result;
    }
    else 
    {
        int pos = 7;   int slen;
        num = rtde_dataspace->rtde_ioctrl_recvbuf[pos];
        rtdeVar->number = num;
        pos++;
        uint8_t type;
        uint32_t code;
        char sname[100];
        int i;
        for(i=0; i<num; i++) 
        {
            slen = rtde_dataspace->rtde_ioctrl_recvbuf[pos];  
            pos++;
            memcpy(sname, &(rtde_dataspace->rtde_ioctrl_recvbuf[pos]), slen);
            sname[slen] = '\0';
            rtdeVar->rtdeVarInfo[i].varNameLen = slen;
            strcpy(rtdeVar->rtdeVarInfo[i].varName, sname);
#if 0            
            printf("output var name: %s\n", sname);
#endif
            pos += slen;//code
            rtdeVar->rtdeVarInfo[i].varId = *(uint32_t *)&(rtde_dataspace->rtde_ioctrl_recvbuf[pos]);
            pos += 4;//type
            rtdeVar->rtdeVarInfo[i].varType = rtde_dataspace->rtde_ioctrl_recvbuf[pos];
            pos++;
        }
    }
#if DEBUG_PRINT    
    printf("Output variables get ok\n");
#endif    
    return result;
}


RtdeResult rtde_input_variables_get(RtdeHandle rtdeHandle,int offset,RtdeVar *rtdeVar)
{
    RtdeResult result = RTDE_ERR_NONE;
    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtdeHandle);
    if(rtde_dataspace == NULL)
    {
        return RTDE_ERR_UNKNOW;
    }

    int len,num;
    *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]) = 3+4;
    rtde_dataspace->rtde_ioctrl_sendbuf[2] = RTDE_GET_CONTROL_INPUTS;
    *(uint32_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[3]) = offset;

    int ssize = socket_send(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_sendbuf, *(uint16_t *)&rtde_dataspace->rtde_ioctrl_sendbuf[0]);
    int rsize = socket_recv(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_recvbuf);

    result = (RtdeResult)rtde_dataspace->rtde_ioctrl_recvbuf[3];
    if(result)//result != 0
    {
        printf("rtde_input_variables_get Error: %d\n", rtde_dataspace->rtde_ioctrl_recvbuf[3]);
        return result;
    }
    else 
    {
        int pos = 7;   int slen;
        num = rtde_dataspace->rtde_ioctrl_recvbuf[pos];
        rtdeVar->number = num;
        pos++;
        uint8_t type;
        uint32_t code;
        char sname[100];
        int i;
        for(i=0; i<num; i++) 
        {
            slen = rtde_dataspace->rtde_ioctrl_recvbuf[pos];  
            pos++;
            memcpy(sname, &(rtde_dataspace->rtde_ioctrl_recvbuf[pos]), slen);
            sname[slen] = '\0';
            rtdeVar->rtdeVarInfo[i].varNameLen = slen;
            strcpy(rtdeVar->rtdeVarInfo[i].varName, sname);
#if DEBUG_PRINT            
            printf("input var name: %s\n", sname);
#endif

            pos += slen;//code
            rtdeVar->rtdeVarInfo[i].varId = *(uint32_t *)&(rtde_dataspace->rtde_ioctrl_recvbuf[pos]);
            pos += 4;//type
            rtdeVar->rtdeVarInfo[i].varType = (rtde_dataspace->rtde_ioctrl_recvbuf[pos]);
            pos++;
        }
    }
#if DEBUG_PRINT    
    printf("Input variables get ok\n");
#endif    
    return result;
}


RtdeResult rtde_control_methods_get(RtdeHandle rtdeHandle,int offset,RtdeFunc *rtdeFunc)
{
    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtdeHandle);
    if(rtde_dataspace == NULL)
    {
        return RTDE_ERR_UNKNOW;
    }
    RtdeResult result = RTDE_ERR_NONE;

    int len,num;
    *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]) = 3+4;
    rtde_dataspace->rtde_ioctrl_sendbuf[2] = RTDE_GET_CONTROL_METHODS;

    *(uint32_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[3]) = offset;

    int ssize = socket_send(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_sendbuf, *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]));
    int rsize = socket_recv(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_recvbuf);

    result = (RtdeResult)rtde_dataspace->rtde_ioctrl_recvbuf[3];
    if(result != 0)//result != 0
    {
        printf("rtde_control_methods_get Error: %d\n", result);
        return result;
    }
    else 
    {
        int pos = 7;   int slen;
        num = rtde_dataspace->rtde_ioctrl_recvbuf[pos];
        rtdeFunc->number = num;
        pos++;
        uint16_t code;
        char sname[100];
        int i;
        for(i=0; i<num; i++) 
        {
            slen = rtde_dataspace->rtde_ioctrl_recvbuf[pos];  
            pos++;
            memcpy(sname, &(rtde_dataspace->rtde_ioctrl_recvbuf[pos]), slen);
            rtdeFunc->rtdeFuncInfo[i].funcNameLen = slen;
            sname[slen] = '\0';
            strcpy(rtdeFunc->rtdeFuncInfo[i].funcName, sname);
#if 0            
            printf("method name : %s\n", sname);
#endif
 
            pos += slen;
            rtdeFunc->rtdeFuncInfo[i].funcId = *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_recvbuf[pos]);
            pos += 2;
        }
    }
#if DEBUG_PRINT    
    printf("Control methods get ok\n");
#endif    
    return result;
}


RtdeResult rtde_all_recipe_get(RtdeHandle rtdeHandle)
{
    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtdeHandle);
    if(rtde_dataspace == NULL)
    {
        return RTDE_ERR_UNKNOW;
    }
    RtdeResult result = RTDE_ERR_NONE;

    RtdeVar output_var;
    RtdeVar input_var;
    RtdeFunc method_func;
    rtde_dataspace->output_vars_num = 0;
    rtde_dataspace->input_vars_num = 0;
    rtde_dataspace->methods_num = 0;
    int offset = 0;

    while(1)
    {
        result = rtde_output_variables_get(rtdeHandle,offset,&output_var);
        if(result != 0)
        {
            return result;
        }
        if(output_var.number == 0)
        {
            break;
        }
        int i;
        for(i=0; i<output_var.number; i++)
        {
            rtde_dataspace->output_vars[rtde_dataspace->output_vars_num].byteslen = rtde_get_type_len(output_var.rtdeVarInfo[i].varType);
            memcpy(rtde_dataspace->output_vars[rtde_dataspace->output_vars_num].varname, output_var.rtdeVarInfo[i].varName, output_var.rtdeVarInfo[i].varNameLen);
            rtde_dataspace->output_vars[rtde_dataspace->output_vars_num].varcode = output_var.rtdeVarInfo[i].varId;
            rtde_dataspace->output_vars[rtde_dataspace->output_vars_num].datatype = output_var.rtdeVarInfo[i].varType;
            rtde_dataspace->output_vars[rtde_dataspace->output_vars_num].used = 0;
            rtde_dataspace->output_vars_num++;
        }
        if(output_var.number < 10) break;
        offset += output_var.number;
    }
    build_output_hash(rtdeHandle,rtde_dataspace->output_vars_num, rtde_dataspace->output_vars);

    offset = 0;
    while(1)
    {
        result = rtde_input_variables_get(rtdeHandle,offset,&input_var);
        if(result != 0)
        {
            return result;
        }    
        if(input_var.number == 0)
        {
            break;
        }
        int i;
        for(i=0; i<input_var.number; i++)
        {
            rtde_dataspace->input_vars[rtde_dataspace->input_vars_num].byteslen = rtde_get_type_len(input_var.rtdeVarInfo[i].varType);
            memcpy(rtde_dataspace->input_vars[rtde_dataspace->input_vars_num].varname, input_var.rtdeVarInfo[i].varName, input_var.rtdeVarInfo[i].varNameLen);
            rtde_dataspace->input_vars[rtde_dataspace->input_vars_num].varcode = input_var.rtdeVarInfo[i].varId;
            rtde_dataspace->input_vars[rtde_dataspace->input_vars_num].datatype = input_var.rtdeVarInfo[i].varType;
            rtde_dataspace->input_vars[rtde_dataspace->input_vars_num].used = 0;
            rtde_dataspace->input_vars_num++;
        }
        if(input_var.number < 10) break;
        offset += input_var.number;
    }
    build_input_hash(rtdeHandle,rtde_dataspace->input_vars_num, rtde_dataspace->input_vars);

    offset = 0;
    while(1)
    {
        result = rtde_control_methods_get(rtdeHandle,offset,&method_func);
        if(result != 0)
        {
            return result;
        }    
        if(method_func.number == 0)
        {
            break;
        }
        int i;
        for(i=0; i<method_func.number; i++)
        {
            rtde_dataspace->methods_list[rtde_dataspace->methods_num].methodcode = method_func.rtdeFuncInfo[i].funcId;
            memcpy(rtde_dataspace->methods_list[rtde_dataspace->methods_num].methodname, method_func.rtdeFuncInfo[i].funcName, method_func.rtdeFuncInfo[i].funcNameLen);
            rtde_dataspace->methods_num++;
        }
        if(method_func.number <=10) break;
        offset += method_func.number;
    }
    build_method_hash(rtdeHandle,rtde_dataspace->methods_num, rtde_dataspace->methods_list);
#if DEBUG_PRINT
    printf("All recipe get ok\n");
#endif    
    return result;
}


RtdeResult rtde_destroy(RtdeHandle rtdeHandle)
{   
    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtdeHandle);
    if(rtde_dataspace == NULL)
    {
        return RTDE_ERR_UNKNOW;
    }
    RtdeResult result = RTDE_ERR_NONE;

    if(socket_close(rtde_dataspace->Socket_fd.sock_control))
    {
        printf("Failed to close control interface\n");
        result = RTDE_ERR_UNKNOW;
    }
    if(socket_close(rtde_dataspace->Socket_fd.sock_receive))
    {
        printf("Failed to close receive interface\n");
        result = RTDE_ERR_UNKNOW;
    }
    if(socket_close(rtde_dataspace->Socket_fd.sock_io_control))
    {
        printf("Failed to close IO control interface\n");
        result = RTDE_ERR_UNKNOW;
    }
    reset_all_hashes(rtdeHandle);
    rtde_dataspace->rtde_recv_state = RTDE_RECV_STATE_IDLE;
    
    rtde_thread_destroy(rtdeHandle);//销毁线程

    free(rtde_dataspace);//释放空间
#if DEBUG_PRINT
    printf("Rtde destroy ok\n");
#endif    
    return result;
}

RtdeResult rtde_input_setup_send(RtdeHandle rtdeHandle,uint16_t number,uint32_t *varId, uint32_t *recipeId)
{
    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtdeHandle);
    if(rtde_dataspace == NULL)
    {
        return RTDE_ERR_UNKNOW;
    }
    RtdeResult result = RTDE_ERR_NONE;

    int pos = 3;//包含header
    if(rtde_dataspace->input_vars_num < number) 
    {
        printf("Error: input_vars_num < varnum\n");
        return RTDE_ERR_UNKNOW;

    }

    *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = number;
    pos += 2;

    // varname's code lis
    int recv_byte = 0;
    int i;
    for (i = 0; i < number; i++)
    {
        *(uint32_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = rtde_dataspace->input_vars[varId[i]].varcode;
        rtde_dataspace->input_data_package[i].data_type = (RtdeVarType)(rtde_dataspace->input_vars[varId[i]].datatype);
        rtde_dataspace->input_data_package[i].type_len = rtde_get_type_len(rtde_dataspace->input_data_package[i].data_type);
        rtde_dataspace->output_vars[varId[i]].used = 1;
        pos += 4;//包长+4bytes
    }
    if((recv_byte+4) > SOCKET_RECEIVE_LEN)
    {
        printf("Error: recv_bytes > SOCKET_RECEIVE_LEN\n");
        result = RTDE_ERR_DATALEN_ULTRALIMIT;
    }
    
    *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]) = pos;
    rtde_dataspace->rtde_ioctrl_sendbuf[2] = RTDE_CONTROL_PACKAGE_SETUP_INPUTS;

    int ssize = socket_send(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_sendbuf, *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]));
    int rsize = socket_recv(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_recvbuf);

    result = (RtdeResult)rtde_dataspace->rtde_ioctrl_recvbuf[3];
    if(result)//result != 0
    {
        printf("Error: %d\n", result);
    }
    *recipeId = rtde_dataspace->rtde_ioctrl_recvbuf[7];
#if DEBUG_PRINT
    printf("Input setup send ok\n");
#endif    
    return result;
}

RtdeResult rtde_output_setup_send(RtdeHandle rtdeHandle,uint16_t number,uint32_t *varId, int frequency, uint32_t *recipeId)
{
    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtdeHandle);
    if(rtde_dataspace == NULL)
    {
        return RTDE_ERR_UNKNOW;
    }

    RtdeResult result = RTDE_ERR_NONE;
    int pos = 3;//包含header

    if(rtde_dataspace->output_vars_num < number) 
    {
        printf("Error: input_vars_num < varnum\n");
        result = RTDE_ERR_UNKNOW;
    }

    *(uint32_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = frequency;
    pos += 4;

    *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = number;
    pos += 2;

    int recv_byte = 0;
    int i;
    for (i = 0; i < number; i++)
    {
        *(uint32_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = rtde_dataspace->output_vars[varId[i]].varcode;
        rtde_dataspace->output_data_package[i].data_type = (RtdeVarType)(rtde_dataspace->output_vars[varId[i]].datatype);
        rtde_dataspace->output_data_package[i].type_len = rtde_get_type_len(rtde_dataspace->output_data_package[i].data_type);
        recv_byte += rtde_dataspace->output_data_package[i].type_len;
        pos += 4;
    }
    if((recv_byte+4) > SOCKET_RECEIVE_LEN)
    {
        printf("Error: recv_bytes > SOCKET_RECEIVE_LEN\n");
        result = RTDE_ERR_DATALEN_ULTRALIMIT;
    }
    
    *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]) = pos;
    rtde_dataspace->rtde_ioctrl_sendbuf[2] = RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS;

    int ssize = socket_send(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_sendbuf, *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]));
    int rsize = socket_recv(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_recvbuf);

    result = (RtdeResult)rtde_dataspace->rtde_ioctrl_recvbuf[3];
    if(result)//result != 0
    {
        printf("rtde_output_setup_send Error: %d\n", result);
    }
    *recipeId = rtde_dataspace->rtde_ioctrl_recvbuf[7];
#if DEBUG_PRINT
    printf("Output setup send ok\n");
#endif    
    return result;
}

RtdeResult rtde_output_start(RtdeHandle rtdeHandle,uint32_t recipeId)
{
    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtdeHandle);
    if(rtde_dataspace == NULL)
    {
        return RTDE_ERR_UNKNOW;
    }
    RtdeResult result = RTDE_ERR_NONE;

    *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]) = 3;
    rtde_dataspace->rtde_ioctrl_sendbuf[2] = RTDE_CONTROL_PACKAGE_START;
    
    int ssize = socket_send(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_sendbuf, *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]));
    int rsize = socket_recv(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_recvbuf);

    result = (RtdeResult)rtde_dataspace->rtde_ioctrl_recvbuf[3];
    if(result)//result != 0
    {
        printf("Error: %d\n", result);
    }

    rtde_dataspace->rtde_recv_state = RTDE_RECV_STATE_RUN;

#if DEBUG_PRINT    
    printf("Output start ok\n");
#endif    
    return result;
}

RtdeResult rtde_output_pause(RtdeHandle rtdeHandle,uint32_t recipeId)
{
    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtdeHandle);
    if(rtde_dataspace == NULL)
    {
        return RTDE_ERR_UNKNOW;    
    }

    RtdeResult result = RTDE_ERR_NONE;

    *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]) = 3;
    rtde_dataspace->rtde_ioctrl_sendbuf[2] = RTDE_CONTROL_PACKAGE_PAUSE;
    
    int ssize = socket_send(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_sendbuf, *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]));
    int rsize = socket_recv(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_recvbuf);

    result = (RtdeResult)rtde_dataspace->rtde_ioctrl_recvbuf[3];
    if(result)//result != 0
    {
        printf("Error: %d\n", result);
    }

    rtde_dataspace->rtde_recv_state = RTDE_RECV_STATE_PAUSE;
#if DEBUG_PRINT
    printf("Output pause ok\n");
#endif    
    return result;
}

RtdeResult rtde_ouput_data_receive(RtdeHandle rtdeHandle,uint32_t recipeId,void *data)
{
    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtdeHandle);
    if(rtde_dataspace == NULL)
    {
        return RTDE_ERR_UNKNOW;
    }
    RtdeResult result = RTDE_ERR_NONE;

    // int rsize = socket_recv(rtde_dataspace->Socket_fd.sock_receive, rtde_dataspace->rtde_recvbuf);
    // if(rsize < 0)
    // {
    //     printf("Error: socket_recv\n");
    //     result = RTDE_ERR_UNKNOW;
    // }    
    // uint8_t package_type = *(uint8_t *)&(rtde_dataspace->rtde_recvbuf[2]);
    // if(package_type != RTDE_DATA_PACKAGE)
    // {
    //     printf("Error: package_type != RTDE_DATA_PACKAGE\n");
    //     result = RTDE_ERR_UNKNOW;
    // }   
    uint16_t data_len = *(uint16_t *)&(rtde_dataspace->rtde_recvbuf[0]);
    // if(data_len > SOCKET_RECEIVE_LEN)
    // {
    //     printf("Error: data_len > SOCKET_RECEIVE_LEN\n");
    //     memcpy(data, "Error: data_len > SOCKET_RECEIVE_LEN", sizeof("Error: data_len > SOCKET_RECEIVE_LEN"));
    //     result = RTDE_ERR_UNKNOW;
    // }   

    memcpy(data, &rtde_dataspace->rtde_recvbuf[0], data_len);
    
    return result;
}


RtdeResult rtde_set_percentvelocity(RtdeHandle rtdeHandle,uint16_t percenVelocity)
{
    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtdeHandle);
    if(rtde_dataspace == NULL)
    {
        return RTDE_ERR_UNKNOW;
    }

    RtdeResult result = RTDE_ERR_NONE;

    int len;
    int pos = 3;
    uint16_t methodCode = find_method_index(rtdeHandle,"setPercentVelocity");

    *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = rtde_dataspace->methods_list[methodCode].methodcode;// method code
    pos += 2;           
    rtde_dataspace->rtde_ioctrl_sendbuf[pos] = 1;    // param num               
    pos += 1;           
    rtde_dataspace->rtde_ioctrl_sendbuf[pos] = DOUBLE;  // param type   
    pos += 1;

    *(double *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = percenVelocity;      
    pos += 8;

    *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]) = pos;

    rtde_dataspace->rtde_ioctrl_sendbuf[2] = RTDE_METHOD_PACKAGE;

    int ssize = socket_send(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_sendbuf, *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]));
    int rsize = socket_recv(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_recvbuf);

    result = (RtdeResult)rtde_dataspace->rtde_ioctrl_recvbuf[3];
    if(rtde_dataspace->rtde_ioctrl_recvbuf[3] != 0)//result != 0
    {
        printf("Error: %d\n", result);
        return result;
    }
#if DEBUG_PRINT
    printf("Set percentvelocity ok\n");
#endif    
    return result;
}



RtdeResult crtde_servoj(RtdeHandle rtdeHandle,int methodCode,double cutoffFreq, double jointPose[6],uint8_t exjNum,double *exjPose)
{
    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtdeHandle);
    if(rtde_dataspace == NULL)
    {
        return RTDE_ERR_UNKNOW;
    }

    RtdeResult result = RTDE_ERR_NONE;

    int len,num;
    int pos = 3;
    rtde_dataspace->rtde_ioctrl_sendbuf[2] = RTDE_METHOD_PACKAGE;//

    *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = rtde_dataspace->methods_list[methodCode].methodcode;// method code
    pos += 2;

    *(uint8_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = 1 + 1 + exjNum;//param num
    pos += 1;

    *(uint8_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = DOUBLE;//param cutoffFreq type
    pos += 1;
    *(double *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = cutoffFreq;
    pos += 8;

    *(uint8_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = VECTOR6D;//param jointPose type double*6
    pos += 1;
    int i;
    for(i=0; i<6; i++)
    {
        *(double *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = jointPose[i];
        pos += 8;
    }

    for(i=0; i<exjNum; i++)
    {
        *(uint8_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = DOUBLE;
        pos += 1;
        *(double *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = exjPose[i];
        pos += 8;
    }

    *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]) = pos;   

    int ssize = socket_send(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_sendbuf, *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]));
    int rsize = socket_recv(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_recvbuf);

    result = (RtdeResult)rtde_dataspace->rtde_ioctrl_recvbuf[3];
    if(result)//result != 0
    {
        printf("crtde_servoj Error: %d\n", result);
        return result;
    }

    num = *(uint32_t *)&(rtde_dataspace->rtde_ioctrl_recvbuf[4]);//函数返回值个数
    // if(num)
    // {   
    //     uint8_t ret_data[100];
    //     RtdeVarType type = (RtdeVarType)*(uint8_t *)&(rtde_dataspace->rtde_ioctrl_recvbuf[8]);
    //     len = rtde_get_type_len(type);
    //     memcpy(ret_data, &(rtde_dataspace->rtde_ioctrl_recvbuf[9]), len*num);
    //     uint32_t ret_val = *(uint32_t *)&ret_data[0];
    // }
#if DEBUG_PRINT
    printf("Set joint pose ok\n");
#endif    
    return result;
}


RtdeResult crtde_servo_stop(RtdeHandle rtdeHandle,int methodCode,double deceleration)
{
    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtdeHandle);
    if(rtde_dataspace == NULL)
    {
        return RTDE_ERR_UNKNOW;
    }
    RtdeResult result = RTDE_ERR_NONE;

    int len,num;
    int pos = 3;

    rtde_dataspace->rtde_ioctrl_sendbuf[2] = RTDE_METHOD_PACKAGE;//

    *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = rtde_dataspace->methods_list[methodCode].methodcode;// method code
    pos += 2;

    *(uint8_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = 1;//param num
    pos += 1;

    *(uint8_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = DOUBLE;//param deceleration type double
    pos += 1;
    *(double *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = deceleration;
    pos += 8;

    *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]) = pos;

    int ssize = socket_send(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_sendbuf, *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]));
    int rsize = socket_recv(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_recvbuf);

    result = (RtdeResult)rtde_dataspace->rtde_ioctrl_recvbuf[3];
    if(result)//result != 0
    {
        printf("Error: %d\n", result);
        return result;
    }
#if DEBUG_PRINT
    printf("Servo stop ok\n");
#endif    
    return result;
}

RtdeResult crtde_resterror(RtdeHandle rtdeHandle,int methodCode)
{
    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtdeHandle);
    if(rtde_dataspace == NULL)
    {
        return RTDE_ERR_UNKNOW;
    }
    RtdeResult result = RTDE_ERR_NONE;

    int len,num;
    int pos = 3;

    rtde_dataspace->rtde_ioctrl_sendbuf[2] = RTDE_METHOD_PACKAGE;//

    *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[pos]) = rtde_dataspace->methods_list[methodCode].methodcode;// method code
    pos += 2;

    pos += 1;

    *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]) = pos;

    int ssize = socket_send(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_sendbuf, *(uint16_t *)&(rtde_dataspace->rtde_ioctrl_sendbuf[0]));
    int rsize = socket_recv(rtde_dataspace->Socket_fd.sock_io_control, rtde_dataspace->rtde_ioctrl_recvbuf);

    result = (RtdeResult)rtde_dataspace->rtde_ioctrl_recvbuf[3];
    if(result)//result != 0
    {
        printf("crtde_resterror Error: %d\n", result);
        return result;
    }
#if DEBUG_PRINT
    printf("Servo stop ok\n");
#endif    
    return result;
}





